#include "flash.h"
#include "stdio.h"
#include <stdint.h>
#include <string.h>


/**
   * @brief  根据输入的地址给出它所在的sector
   *         例如：
            uwStartSector = GetSector(FLASH_USER_START_ADDR);
            uwEndSector = GetSector(FLASH_USER_END_ADDR);
   * @param  Address：地址
   * @retval 地址所在的sector
   */
static uint32_t GetSector(uint32_t Address)
{
    uint32_t sector = 0;

    if ((Address < ADDR_FLASH_SECTOR_1) && (Address >= ADDR_FLASH_SECTOR_0)) {
        sector = FLASH_SECTOR_0;
    } else if ((Address < ADDR_FLASH_SECTOR_2) && (Address >= ADDR_FLASH_SECTOR_1)) {
        sector = FLASH_SECTOR_1;
    } else if ((Address < ADDR_FLASH_SECTOR_3) && (Address >= ADDR_FLASH_SECTOR_2)) {
        sector = FLASH_SECTOR_2;
    } else if ((Address < ADDR_FLASH_SECTOR_4) && (Address >= ADDR_FLASH_SECTOR_3)) {
        sector = FLASH_SECTOR_3;
    } else if ((Address < ADDR_FLASH_SECTOR_5) && (Address >= ADDR_FLASH_SECTOR_4)) {
        sector = FLASH_SECTOR_4;
    } else if ((Address < ADDR_FLASH_SECTOR_6) && (Address >= ADDR_FLASH_SECTOR_5)) {
        sector = FLASH_SECTOR_5;
    } else if ((Address < ADDR_FLASH_SECTOR_7) && (Address >= ADDR_FLASH_SECTOR_6)) {
        sector = FLASH_SECTOR_6;
    } else { /*(Address < FLASH_END_ADDR) && (Address >= ADDR_FLASH_SECTOR_23))*/
        sector = FLASH_SECTOR_7;
    }
    return sector;
}

/**
* Length: Data数据长度（单位：Byte）
*/
HAL_StatusTypeDef STM32Flash_Write(uint32_t Address, uint8_t* Data, uint32_t Length)
{
    HAL_StatusTypeDef status;
    uint32_t sectorError = 0;
    FLASH_EraseInitTypeDef eraseInitStruct;

    if (Address < FLASH_USER_START_ADDR || (Address + Length) > FLASH_USER_END_ADDR || (Address % 32) != 0) {
        return HAL_ERROR; // Address out of range
    }


    HAL_FLASH_Unlock();

    eraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS;
    eraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3;
    eraseInitStruct.Banks = FLASH_BANK_1; // Assuming single bank operation
    eraseInitStruct.Sector = GetSector(Address);
    eraseInitStruct.NbSectors = GetSector(Address + Length) - eraseInitStruct.Sector + 1;
    status = HAL_FLASHEx_Erase(&eraseInitStruct, &sectorError);
    
    if (status != HAL_OK) {
        HAL_FLASH_Lock();
        return status;
    }

    uint8_t data_write_tmp[32] = {0};  // 每次写入32个字节（256bit）
    uint32_t i = 0, j = 0;
    // STM32H7x3要求每次写入256bit（32个字节），所以需要分批写入
    for (; i < Length; i += 32) {
        for(j = 0; j < 32 && i+j < Length; j++) {
            data_write_tmp[j] = Data[i+j]; // Extract each nibble
        }
        if(j < 32) {
            for(; j < 32; j++) {
                data_write_tmp[j] = 0; // Fill remaining with zeros
            }
        }
        status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, Address, (uint32_t)data_write_tmp);
        Address += 32;
    }

    // HAL_Delay(10);
    HAL_FLASH_Lock();
    return HAL_OK;
}

HAL_StatusTypeDef STM32Flash_Read(uint32_t Address, uint8_t* Data, uint32_t Length)
{
    if (Address < FLASH_USER_START_ADDR || (Address + Length) > FLASH_USER_END_ADDR) {
        return HAL_ERROR; // Address out of range
    }

    // for (uint32_t i = 0; i < Length; i++) {
    //     Data[i] = *(uint8_t*)(Address + i);
    // }
    memcpy(Data, (uint8_t*)Address, Length);

    return HAL_OK;
}
